import re
import numpy as np
import time
import pygame
import matplotlib.pyplot as plt
from bezier_curve_smooth import my_bezier_curve


# 行列数
cols = 52
rows = 52
brick_len = 10
col_length = cols * brick_len
row_length = rows * brick_len
screen = pygame.display.set_mode((col_length, row_length))


# 颜色
red = (255, 0, 0)
green = (0, 255, 0)
blue = (0, 0, 255)
grey = (120, 120, 120)
# grey = (230, 230, 230)
yellow = (255, 255, 0)
black = (0, 0, 0)
white = (230, 230, 230)
# 格子长宽
w = h = brick_len
grid_width = 2


def set_param(_row, _col,  _brick_len):
    global cols, rows, brick_len, col_length, row_length, screen, w, h
    cols = _col + 2
    rows = _row + 2
    brick_len = _brick_len
    w = h = brick_len
    col_length = cols * brick_len
    row_length = rows * brick_len
    screen = pygame.display.set_mode((row_length, col_length))

class spot:
    def __init__(self, x, y, obs=False):
        # 坐标
        self.i = x
        self.j = y
        # 是不是障碍
        self.obs = False
        # 选择状态
        self.closed = False

    def show(self, color, st):
        global grid_width
        if self.closed == False:
            # color是RGB元组， 位置元组分别是x,y坐标， w,h是宽高， st是线宽，0表示填充，其他表示线宽
            pygame.draw.rect(screen, color, (self.i * w, self.j * h, w, h), st)
            if grid_width:
                pygame.draw.rect(screen, black, (self.i * w, self.j * h, w, h), grid_width)

            pygame.display.update()

    def path(self, color, st, gird_width=1):
        pygame.draw.rect(screen, color, (self.i * w, self.j * h, w, h), st)
        if grid_width:
            pygame.draw.rect(screen, black, (self.i * w, self.j * h, w, h), gird_width)
        pygame.display.update()


def draw_center(gird_list, color=black, st=1, dx=1, dy=1):
    for _grid in gird_list:
        x = _grid[0] + dx + 0.5
        y = _grid[1] + dy + 0.5
        pygame.draw.rect(screen, black, (x*brick_len, y*brick_len, 5*st, 5*st), 0)
    pygame.draw.rect(screen, black, (2.5*100, 2.5*100, 20, 20), 10)


def create_map_grid(map_info):
    # 创建节点
    grid = [0 for i in range(rows)]
    for i in range(rows):
        grid[i] = [0 for j in range(cols)]

    for i in range(rows):
        for j in range(cols):
            grid[i][j] = spot(i, j)     # 坐标已考虑边界

    for i in range(len(map_info)):
        for j in range(len(map_info[0])):
            # 0是黑色，表示障碍物
            if map_info[i][j] == 0:
                grid[i+1][j+1].obs = True
                grid[i+1][j+1].show((0, 0, 0), 0)
            else:
                grid[i+1][j+1].show(white, 0)     # 带边框的白色

    # 画围墙
    for i in range(rows):
        grid[i][0].show(grey, 0)
        grid[i][cols - 1].show(grey, 0)
        grid[i][0].obs = True
        grid[i][cols - 1].obs = True

    for i in range(cols):
        grid[0][i].show(grey, 0)
        grid[rows - 1][i].show(grey, 0)
        grid[0][i].obs = True
        grid[rows - 1][i].obs = True

    return grid


# 读取行列坐标信息
def load_processed_grid():
    pattern = re.compile(r"current pos is (\d+)， (\d+)")
    pos = []
    with open('txt_data/data050702', 'r',  encoding='utf-8') as file:
        context = file.read()
        matches = pattern.findall(context)
        for match in matches:
            pos.append([int(match[0]), int(match[1])])
    return pos


def draw_astar(grid, path, pos):
    # 画起点和终点
    grid[path[0][0]+1][path[0][1]+1].show(red, 0)
    grid[path[-1][0]+1][path[-1][1]+1].show(red, 0)
    # 遍历所有的行和列
    for i in range(1, len(pos)):
        # 将对应的格点设置为黄色
        grid[pos[i-1][0]+1][pos[i-1][1]+1].show(blue, 0)
        grid[pos[i][0] + 1][pos[i][1] + 1].show(yellow, 0)
        # 暂停0.1秒
        pygame.time.delay(100)
    # 画路径
    for i in range(0, len(path)):
        grid[path[i][0]+1][path[i][1]+1].show((0, 255, 0), 0)


def main():
    grid_np = np.load('npy/resized_map.npy')
    grid = create_map_grid(grid_np)
    path = np.load('npy/astar_path.npy')
    processed_grid = load_processed_grid()
    draw_astar(grid, path, processed_grid)

    # 保持窗口打开直到点击结束
    running = True
    while running:
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                running = False
    pygame.quit()


# 画论文用示意图，存在多栅格同价值
def test1():
    global cols, rows, brick_len,col_length, row_length, screen, w, h
    cols = 12
    rows = 16

    brick_len = 50
    w = h = brick_len
    col_length = cols * brick_len
    row_length = rows * brick_len
    screen = pygame.display.set_mode((row_length, col_length))

    grid_npy = np.full((14, 10), 255)
    grid = create_map_grid(grid_npy)
    path_tmp = [(1, 1), (12, 8)]
    path = np.array(path_tmp)
    draw_astar(grid, path, [])
    black_grid = [(6,8),(7,8),(8,8),(8,7),(8,6)]
    for tmp in black_grid:
        grid[tmp[0]][tmp[1]].show(black, 0)

    tmp_color = (4*16+13, 7*16+1, 255)
    tmp_color2 = (15*16+6, 15*16+11, 69)
    for i in range(3, 7):
        grid[i][i].show(tmp_color, 0)
        grid[i-1][i].show(tmp_color2, 0)
        grid[i][i-1].show(tmp_color2, 0)

    grid[7][7].show(tmp_color, 0)

    # 保持窗口打开直到点击结束
    running = True
    while running:
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                running = False
    pygame.quit()


# 论文画图，示意格点删减
def test2():
    global cols, rows, brick_len, col_length, row_length, screen, w, h
    cols = 6
    rows = 8

    brick_len = 100
    w = h = brick_len
    col_length = cols * brick_len
    row_length = rows * brick_len
    screen = pygame.display.set_mode((row_length, col_length))

    grid_npy = np.full((6, 4), 255)
    grid = create_map_grid(grid_npy)
    path_tmp = [(3,0),(0,3),(5,0)]
    path = np.array(path_tmp)
    draw_astar(grid, path, [])
    # black_grid = [(6, 8), (7, 8), (8, 8), (8, 7), (8, 6)]
    # for tmp in black_grid:
    #     grid[tmp[0]][tmp[1]].show(black, 0)

    grid[4][3].show(black, 0)
    grid[4][4].show(black, 0)

    # 保持窗口打开直到点击结束
    running = True
    while running:
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                running = False
    pygame.quit()


# 论文画图，bezier平滑后曲线的样子
def test3():
    global grid_width
    grid_width = 0
    grid_np = np.load('npy/resized_map.npy')
    grid = create_map_grid(grid_np)
    path_tmp = [[20,20,20,20,20,19,18,17,16,16,16,16,15,14,13,12,11,10,9,8,7,7,7,7,7,7,7,7,6,5,4,4,4,4,4,4,5,5,5,5,5,5,5,5,5,4,3,2,1,0]
,[47,46,45,44,43,42,41,40,39,38,37,36,35,34,33,33,32,32,31,30,29,28,27,26,25,24,23,22,21,20,19,18,17,16,15,14,13,12,11,10,9,8,7,6,5,4,3,2,1,0]]
    path_array = []
    for i in range(1, len(path_tmp[0])+1):
        path_array.append((path_tmp[0][-i], path_tmp[1][-i]))
        print((path_tmp[0][-i], path_tmp[1][-i]))

    path = np.array(path_array)
    processed_grid = []

    draw_astar(grid, path, processed_grid)

    # 保持窗口打开直到点击结束
    running = True
    while running:
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                running = False
    pygame.quit()

    bezier_smooth_f = np.load('npy/bezier_curve_50')
    bezier_smooth_int = []


# 论文画图，折线与曲线对比，使用plt模块
def test4():
    # # 生成一个随机的二维矩阵作为示例
    matrix = np.load('npy/resized_map_60_80.npy')

    # 定义轨迹路径

    # path_array = []
    # for i in range(1, len(path_tmp[0]) + 1):
    #     path_array.append((path_tmp[0][-i], path_tmp[1][-i]))

    # 绘制栅格地图
    matrix_inverted = np.where(matrix == 0, 255, np.where(matrix == 255, 0, matrix))
    plt.imshow(matrix_inverted, cmap='binary', interpolation='nearest')

    # 绘制轨迹
    path = np.load('txt_data/path_60_80_no.npy')
    x_path = []
    y_path = []
    for item in path:
        x_path.append(item[0])
        y_path.append(item[1])
    plt.plot(y_path, x_path, color='blue', label='broken line path')

    # breakpoints = [(0, 0), (7, 29), (10, 32), (13, 33), (16, 36), (16, 39), (20, 43), (20, 47)]
    # break_x = []
    # break_y = []
    # for item in breakpoints:
    #     break_x.append(item[0])
    #     break_y.append(item[1])
    # # plt.plot(break_y, break_x, color='yellow')

    final_points = [(20, 20), (30, 28), (38, 36), (38, 38), (41, 44), (54, 57), (54, 60), (50, 70)]
    curve_points = my_bezier_curve(final_points)
    x_curve = [final_points[0][0]]
    y_curve = [final_points[0][0]]
    for item in curve_points:
        x_curve.append(item[0])
        y_curve.append(item[1])
    x_curve.append(final_points[-1][0])
    y_curve.append(final_points[-1][1])

    # 定义曲线上的点坐标
    # bezier_curve = np.load('npy/bezier_curve_50.npy')
    # bezier_curve = np.transpose(bezier_curve)
    # # print(bezier_curve)
    # x_curve = np.insert(bezier_curve[:][0],0, breakpoints[0][0])
    # y_curve = np.insert(bezier_curve[:][1], 0, breakpoints[0][1])

    # 绘制曲线
    plt.plot(y_curve, x_curve, color='red', label='optimized curve path')
    plt.legend()

    plt.gca().invert_yaxis()  # 翻转y轴以匹配矩阵索引顺序

    plt.show()


if __name__ == '__main__':
    # main()
    # test2()
    test4()

